//-----------------------------------------------------------------------------
//
// @file	Quaternion.cpp
// @brief	Quaternion  classes
// @author	Vinod Melapudi
// @date	20Feb2010
//
//-----------------------------------------------------------------------------

#include "Quaternion.h"

Quaternion::Quaternion( const Vector3& _v, float _w ) :
	v(_v), w(_w)
{
}

void Quaternion::Set( const Vector3& _v, float _w )
{
	v = _v;
	w = _w;
}

void Quaternion::FromAngleAxis( const Vector3& axis, float angle )
{
	float halfAngle = angle/2.0f;
	v = axis.normalize() * sin( halfAngle );
	w = cos( halfAngle );
}

float Quaternion::normsqr() const
{
	return v*v + w*w;
}

float Quaternion::norm() const
{
	return sqrt( normsqr() );
}

Quaternion Quaternion::normalize() const
{
	Quaternion qOut = (*this) / norm();
	return *this;
}

Quaternion Quaternion::operator* ( float s ) const
{
	return Quaternion( v*s, w*s );
}

Quaternion Quaternion::operator/ ( float s ) const
{
	return Quaternion( v/s, w/s );
}

Quaternion Quaternion::operator* ( const Quaternion& q ) const
{
	return Quaternion( v%q.v + v*q.w + q.v*w, w*q.w - v*q.v );
}

Quaternion Quaternion::Inverse() const
{
	return Quaternion( -v, w ) / normsqr();
}

Vector3 Quaternion::Rotate( const Vector3& v ) const
{
	Quaternion qOut = (*this) * Quaternion(v) * Inverse();
	return qOut.v;
}